
#include <Arduino.h>
#include <Servo.h>

#include "app_config.hpp"


static Servo servo_lf;
static Servo servo_lr;
static Servo servo_rf;
static Servo servo_rr;


static bool _motion_completed = true;

static bool _body_may_be_steady = true;


bool motion_completed() {
    return _motion_completed;
}


static TaskIndexType wait_for_steady_index = SchedulerType::INVALID_INDEX;


TimeType task_wait_for_steady() {
    _body_may_be_steady = true;
    wait_for_steady_index = SchedulerType::INVALID_INDEX;
    serial_log.println("Body Steady");
    return 0;
}


bool is_body_steady() {
    return _body_may_be_steady;
}


static void set_motion_completed(bool flag) {
    _motion_completed = flag;
    if(flag) {
        if(wait_for_steady_index == SchedulerType::INVALID_INDEX) {
            wait_for_steady_index = delay_callback.add_task(task_wait_for_steady, 1000);
        }
        else {
            delay_callback.reset_task(wait_for_steady_index, 1000);
        }
    }
    else {
        _body_may_be_steady = false;
        if(wait_for_steady_index != SchedulerType::INVALID_INDEX) {
            delay_callback.remove_task(wait_for_steady_index);
        }
    }
    
}

void init_servo() {
    // 配置舵机PWM，500µs=0度，2500µs=180度
    servo_lf.attach(PIN_SERVO_LF, 500, 2500);
    servo_lr.attach(PIN_SERVO_LR, 500, 2500);
    servo_rf.attach(PIN_SERVO_RF, 500, 2500);
    servo_rr.attach(PIN_SERVO_RR, 500, 2500);
}


// TODO: 实测舵机编号到底对应哪个位置
#define servo1 servo_rf
#define servo3 servo_lf

#define servo2 servo_rr
#define servo4 servo_lr


/**
 * @brief 若rel 为正，表示向前转动abs(rel)，反之为向后转。0 表示90°，腿转到垂直。
 *
 * @param sv
 * @param rel
 */
void servo_step(Servo &sv, int rel) {
    Servo *psv = &sv;
    if (psv == &servo_lf || psv == &servo_lr) {
        // 左侧舵机角度正方向向后，右侧向前，所把左侧的角度取反
        rel = 0 - rel;
    }

    int angle = 90 + rel;
    if (rel > 90) {
        angle = 180;
    }
    else if (rel < -90) {
        angle = 0;
    }
    // serial_log.print("ServoMove = ");
    // serial_log.print(rel);
    // serial_log.print('\n');
    sv.write(angle);
}


// 舵机运动函数都改成定时回调任务，内部用状态机实现，消除长时间延时死等。
// 只有在motion_completed 为true 时，才能发起新的运动


/**
 * @brief 向前走的定时任务，添加任务时，delay_time 设为0，由函数自行设置延时时间
 *
 * 不可重复添加同一个函数到任务列表，必须先检查运动序列有没有完成。
 *
 * @return TimeType
 */
TimeType task_step_forward() {
    static u8 state = 0;
    if (state == 0) {
        if (!motion_completed()) {
            // 前一个运动还没完成时，不启动新的运动序列
            return 0;
        }
        else {
            // 启动序列，设置标志
            set_motion_completed(false);
        }
    }

    switch (state) {
        case 0:
            servo_step(servo_lf, 50);
            servo_step(servo_rr, 50);
            servo_step(servo_lr, -20);
            servo_step(servo_rf, -20);
            break;
        case 1:
            servo_step(servo_lf, -20);
            servo_step(servo_rr, -20);
            servo_step(servo_lr, 50);
            servo_step(servo_rf, 50);
            break;
        case 2:
            servo_step(servo_lf, 50);
            servo_step(servo_rr, 50);
            servo_step(servo_lr, -20);
            servo_step(servo_rf, -20);
            break;
        case 3:
            servo_step(servo_lf, -20);
            servo_step(servo_rr, -20);
            servo_step(servo_lr, 50);
            servo_step(servo_rf, 50);
            break;
        case 4:
            // 序列执行完成，但还要再延时100ms 才设置标志
            // 不然两个序列可能会无缝衔接
            servo_step(servo_lf, 20);
            servo_step(servo_rf, 20);
            servo_step(servo_lr, -20);
            servo_step(servo_rr, -20);
            break;
        case 5:
            set_motion_completed(true);
            state = 0;
            return 0;
    }

    ++state;
    return 200;
    // 每一步之间延迟100 毫秒
    // 时间源应该是毫秒类型
}


/**
 * @brief 向后的定时任务，添加任务时，delay_time 设为0，由函数自行设置延时时间
 *
 * @return TimeType
 */
TimeType task_step_backward() {
    static u8 state = 0;
    if (state == 0) {
        if (!motion_completed()) {
            // 前一个运动还没完成时，不启动新的运动序列
            return 0;
        }
        else {
            // 启动序列，设置标志
            set_motion_completed(false);
        }
    }

    switch (state) {
        case 0:
            servo3.write(140);
            servo2.write(40);
            break;
        case 1:
            servo4.write(40);
            servo1.write(140);
            break;
        case 2:
            servo3.write(90);
            servo2.write(90);
            break;
        case 3:
            servo4.write(90);
            servo1.write(90);
            break;
        case 4:
            servo4.write(140);
            servo1.write(40);
            break;
        case 5:
            servo3.write(40);
            servo2.write(140);
            break;
        case 6:
            servo4.write(90);
            servo1.write(90);
            break;
        case 7:
            // 序列执行完成
            servo3.write(90);
            servo2.write(90);
            break;
        case 8:
            set_motion_completed(true);
            state = 0;
            return 0;
    }

    ++state;
    return 100;
}


/**
 * @brief 右转
 *
 * @return TimeType
 */
TimeType task_turn_right() {
    static int state = 0;
    static u8 loop_counter = 0;

    if (loop_counter == 0 && state == 0) {
        if (!motion_completed()) {
            // 前一个运动还没完成时，不启动新的运动序列
            return 0;
        }
        else {
            // 启动序列，设置标志
            set_motion_completed(false);
        }
    }

    switch (state) {
        case 0:
            servo1.write(100);
            servo4.write(100);
            break;
        case 1:
            servo3.write(60);
            servo2.write(60);
            break;
        case 2:
            servo1.write(140);
            servo4.write(140);
            break;
        case 3:
            servo3.write(40);
            servo2.write(40);
            break;
        case 4:
            servo3.write(90);
            servo2.write(90);
            servo1.write(90);
            servo4.write(90);
            break;
        case 5:
            servo1.write(80);
            servo4.write(80);
            break;
        case 6:
            servo3.write(120);
            servo2.write(120);
            break;
        case 7:
            servo1.write(90);
            servo4.write(90);
            break;
        case 8:
            servo3.write(140);
            servo2.write(140);
            break;
        case 9:
            // 执行完一次
            servo3.write(90);
            servo2.write(90);
            ++loop_counter;
            if (loop_counter == 3) {
                // 3 次执行完成
                loop_counter = 0;
            }
            else {
                // 否则回到第一步循环执行
                state = -1;
            }
            break;
        case 10:
            set_motion_completed(true);
            return 0;
    }

    ++state;
    return 100;
}


/**
 * @brief 左转
 *
 * @return TimeType
 */
TimeType task_turn_left() {
    static int state = 0;
    static u8 loop_counter = 0;

    if (loop_counter == 0 && state == 0) {
        if (!motion_completed()) {
            // 前一个运动还没完成时，不启动新的运动序列
            return 0;
        }
        else {
            // 启动序列，设置标志
            set_motion_completed(false);
        }
    }

    switch (state) {
        case 0:
            servo1.write(80);
            servo4.write(80);
            break;
        case 1:
            servo3.write(120);
            servo2.write(120);
            break;
        case 2:
            servo1.write(40);
            servo4.write(40);
            break;
        case 3:
            servo3.write(140);
            servo2.write(140);
            break;
        case 4:
            servo3.write(90);
            servo2.write(90);
            servo1.write(90);
            servo4.write(90);
            break;
        case 5:
            servo1.write(100);
            servo4.write(100);
            break;
        case 6:
            servo3.write(60);
            servo2.write(60);
            break;
        case 7:
            servo1.write(90);
            servo4.write(90);
            break;
        case 8:
            servo3.write(40);
            servo2.write(40);
            break;
        case 9:
            // 执行完一次
            servo3.write(90);
            servo2.write(90);
            ++loop_counter;
            if (loop_counter == 3) {
                // 3 次执行完成
                loop_counter = 0;
            }
            else {
                // 否则回到第一步循环执行
                state = -1;
            }
            break;
        case 10:
            state = 0;
            set_motion_completed(true);
            return 0;
    }

    ++state;
    return 100;
}


TimeType task_sit_down() {
    static u8 state = 0;
    if (state == 0) {
        if (!motion_completed()) {
            // 前一个运动还没完成时，不启动新的运动序列
            return 0;
        }
        else {
            // 启动序列，设置标志
            set_motion_completed(false);
        }
    }

    switch (state) {
        case 0:
            servo2.write(140);
            servo4.write(40);
            break;

        case 1:
            servo2.write(90);
            servo4.write(90);
            break;
        case 2:
            state = 0;
            set_motion_completed(true);
            return 0;
    }

    ++state;
    return 3000;
}


TimeType task_lie_down() {
    static u8 state = 0;
    if (state == 0) {
        if (!motion_completed()) {
            // 前一个运动还没完成时，不启动新的运动序列
            return 0;
        }
        else {
            // 启动序列，设置标志
            set_motion_completed(false);
        }
    }

    switch (state) {
        case 0:
            servo1.write(180);
            servo3.write(0);
            servo2.write(0);
            servo4.write(180);
            break;

        case 1:
            servo1.write(90);
            servo3.write(90);
            servo2.write(90);
            servo4.write(90);
            break;

        case 2:
            state = 0;
            set_motion_completed(true);
            return 0;
    }

    ++state;
    return 3000;
}


/**
 * @brief 抬左手
 *
 * @return TimeType
 */
TimeType task_shake_left_hand() {
    static int state = 0;
    static u8 loop_counter = 0;

    if (loop_counter == 0 && state == 0) {
        if (!motion_completed()) {
            // 前一个运动还没完成时，不启动新的运动序列
            return 0;
        }
        else {
            // 启动序列，设置标志
            set_motion_completed(false);
        }
    }

    switch (state) {
        case 0:
            servo3.write(0);
            break;
        case 1:
            servo3.write(30);
            // 执行完一次
            ++loop_counter;
            if (loop_counter == 3) {
                // 3 次执行完成
                loop_counter = 0;
            }
            else {
                // 否则回到第一步循环执行
                state = -1;
            }
            break;
        case 2:
            servo3.write(90);
            break;
        case 3:
            state = 0;
            set_motion_completed(true);
            return 0;
    }

    ++state;
    return 500;
}


/**
 * @brief 抬右手
 *
 * @return TimeType
 */
TimeType task_shake_right_hand() {
    static int state = 0;
    static u8 loop_counter = 0;

    if (loop_counter == 0 && state == 0) {
        if (!motion_completed()) {
            // 前一个运动还没完成时，不启动新的运动序列
            return 0;
        }
        else {
            // 启动序列，设置标志
            set_motion_completed(false);
        }
    }

    switch (state) {
        case 0:
            servo1.write(180);
            break;
        case 1:
            servo1.write(150);
            // 执行完一次
            ++loop_counter;
            if (loop_counter == 3) {
                // 3 次执行完成
                loop_counter = 0;
            }
            else {
                // 否则回到第一步循环执行
                state = -1;
            }
            break;
        case 2:
            servo1.write(90);
            break;
        case 3:
            state = 0;
            set_motion_completed(true);
            return 0;
    }

    ++state;
    return 500;
}


TimeType task_do_sleep() {
    int state = 0;
    if (state == 0) {
        if (!motion_completed()) {
            // 前一个运动还没完成时，不启动新的运动序列
            return 0;
        }
        else {
            // 启动序列，设置标志
            set_motion_completed(false);
        }
    }

    switch (state) {
        case 0:
            servo1.write(0);
            servo3.write(180);
            servo2.write(180);
            servo4.write(0);
            break;
        case 1:
            state = 0;
            set_motion_completed(true);
            return 0;
    }

    ++state;
    return 3000;
}


/**
 * @brief 左右抖动身体
 *
 * @return TimeType
 */
TimeType task_shake_body() {
    static int state = 0;
    static u8 loop_counter = 0;

    if (loop_counter == 0 && state == 0) {
        if (!motion_completed()) {
            // 前一个运动还没完成时，不启动新的运动序列
            return 0;
        }
        else {
            // 启动序列，设置标志
            set_motion_completed(false);
        }
    }

    switch (state) {
        case 0:
            servo_step(servo_lf, 30);
            servo_step(servo_lr, -30);
            break;
        case 1:
            servo_step(servo_lf, 60);
            servo_step(servo_lr, -60);
            break;
        case 2:
            servo_step(servo_lf, 30);
            servo_step(servo_lr, -30);
            break;
        case 3:
            servo_step(servo_lf, 10);
            servo_step(servo_lr, -10);
            servo_step(servo_rf, 10);
            servo_step(servo_rr, -10);
            break;
        case 4:
            servo_step(servo_rf, 30);
            servo_step(servo_rr, -30);
            break;
        case 5:
            servo_step(servo_rf, 60);
            servo_step(servo_rr, -60);
            break;
        case 6:
            servo_step(servo_rf, 30);
            servo_step(servo_rr, -30);
            break;
        case 7:
            // 执行完一次
            servo_step(servo_rf, 10);
            servo_step(servo_rr, -10);
            ++loop_counter;
            if (loop_counter == 3) {
                // 3 次执行完成
                loop_counter = 0;
            }
            else {
                // 否则回到第一步循环执行
                state = -1;
            }
            break;
        case 10:
            state = 0;
            set_motion_completed(true);
            return 0;
    }

    ++state;
    return 200;
}


/**
 * @brief 依次让左前、左后、右前、右后的舵机向前动一下
 *
 * @return TimeType
 */
TimeType task_servo_test() {
    static u8 state = 0;
    if (state == 0) {
        if (!motion_completed()) {
            // 前一个运动还没完成时，不启动新的运动序列
            return 0;
        }
        else {
            // 启动序列，设置标志
            set_motion_completed(false);
        }
    }

    switch (state) {
        case 0:
            servo_step(servo_lf, 50);
            break;
        case 1:
            servo_step(servo_lf, 0);
            break;
        case 2:
            servo_step(servo_lr, 50);
            break;
        case 3:
            servo_step(servo_lr, 0);
            break;
        case 4:
            servo_step(servo_rf, 50);
            break;
        case 5:
            servo_step(servo_rf, 0);
            break;
        case 6:
            servo_step(servo_rr, 50);
            break;
        case 7:
            // 序列执行完成
            servo_step(servo_rr, 0);
            break;
        case 8:
            set_motion_completed(true);
            state = 0;
            return 0;
    }

    ++state;
    return 600;
}